// --------------------------------------------------------------------------------------------------------------------
// <copyright file="AX12DualGripperAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Runtime.Robotis
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Hardware.Robotiq;
    using Microsoft.Robotics.Hardware.Robotis;
    using Microsoft.Robotics.Hardware.Runtime.Robotiq;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// AX12 Dynamixel based gripper agent.  For now reuses RobotIQ S Model gripper state and control messages.
    /// Only positions are supported.
    /// </summary>
    [System.Diagnostics.CodeAnalysis.SuppressMessage(
        "Microsoft.Design",
        "CA1001:TypesThatOwnDisposableFieldsShouldBeDisposable",
        Justification = "Agent handles disposal of unmanaged resources in OnDeactivate()")]
    [DataContract]
    public class AX12DualGripperAgent :
        ConsumerProducerAgent<TimerMessage, RobotiqSModelStateMessage>,
        IControlReceiver<RobotiqSModelControlMessage>
    {
        /// <summary>
        /// Default left servo ID
        /// </summary>
        public const int DefaultLeftServoId = 2;

        /// <summary>
        /// Default right servo ID
        /// </summary>
        public const int DefaultRightServoId = 1;

        /// <summary>
        /// Default left servo full open position
        /// </summary>
        public const int DefaultLeftFullOpenPosition = 509;

        /// <summary>
        /// Default right servo full open position
        /// </summary>
        public const int DefaultRightFullOpenPosition = 522;

        /// <summary>
        /// Default left servo full close position
        /// </summary>
        public const int DefaultLeftFullClosePosition = 725;

        /// <summary>
        /// Default right servo full close position
        /// </summary>
        public const int DefaultRightFullClosePosition = 315;

        /// <summary>
        /// Default move speed
        /// </summary>
        public const int DefaultMoveSpeed = 80;

        /// <summary>
        /// Port number
        /// </summary>
        [DataMember]
        private int portNumber;

        /// <summary>
        /// Baud number
        /// </summary>
        [DataMember]
        private int baudNumber;

        /// <summary>
        /// ID of left servo
        /// </summary>
        [DataMember]
        private int leftServoId;

        /// <summary>
        /// ID of right servo
        /// </summary>
        [DataMember]
        private int rightServoId;

        /// <summary>
        /// Move speed of servos
        /// </summary>
        [DataMember]
        private int moveSpeed;

        /// <summary>
        /// USB2Dynamixel interface.
        /// </summary>
        private Dynamixel dynamixel;

        /// <summary>
        /// Initializes a new instance of the <see cref="AX12DualGripperAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">Producer agent.</param>
        public AX12DualGripperAgent(string name, IProducer<TimerMessage> producer)
            : base(name, producer.Name)
        {
            this.portNumber = Dynamixel.DefaultPortNumber;
            this.baudNumber = Dynamixel.DefaultBaudNumber;
            this.leftServoId = DefaultLeftServoId;
            this.rightServoId = DefaultRightServoId;
            this.moveSpeed = DefaultMoveSpeed;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="AX12DualGripperAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">Producer agent.</param>
        /// <param name="portNumber">COM port number of USB2Dynamixel</param>
        /// <param name="baudNumber">Baud number.</param>
        /// <param name="leftServoId">ID of left servo.</param>
        /// <param name="rightServoId">ID of right servo.</param>
        /// <param name="moveSpeed"> Servo move speed. </param>
        public AX12DualGripperAgent(
            string name,
            IProducer<TimerMessage> producer,
            int portNumber,
            int baudNumber,
            int leftServoId,
            int rightServoId,
            int moveSpeed)
            : base(name, producer.Name)
        {
            this.portNumber = portNumber;
            this.baudNumber = baudNumber;
            this.leftServoId = leftServoId;
            this.rightServoId = rightServoId;
            this.moveSpeed = moveSpeed;
        }

        /// <summary>
        /// Initializes the agent
        /// </summary>
        /// <param name="locator">Agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.dynamixel = new Dynamixel(this.portNumber, this.baudNumber);
            this.dynamixel.Connect();
            this.dynamixel.SetMoveSpeed(this.leftServoId, this.moveSpeed);
            this.dynamixel.SetMoveSpeed(this.rightServoId, this.moveSpeed);
        }

        /// <summary>
        /// Handle command message.  Only position commands currently supported.
        /// </summary>
        /// <param name="message"> Control message.</param>
        public void ReceiveControl(RobotiqSModelControlMessage message)
        {
            double percentageClosed = this.ClampPercentage(((double)message.Command.Position) / 255.0);

            int leftServoCommand = DefaultLeftFullOpenPosition + (int)(percentageClosed *
                (DefaultLeftFullClosePosition - DefaultLeftFullOpenPosition));

            int rightServoCommand = DefaultRightFullOpenPosition -
                (int)(percentageClosed * (DefaultRightFullOpenPosition - DefaultRightFullClosePosition));

            this.dynamixel.SetPosition(this.rightServoId, rightServoCommand);
            this.dynamixel.SetPosition(this.leftServoId, leftServoCommand);
        }

        /// <summary>
        /// Called when the agent receives a message.
        /// </summary>
        /// <param name="message">The received message.</param>
        public override void Receive(TimerMessage message)
        {
            int leftServoPosition = this.dynamixel.GetPosition(this.leftServoId);
            int rightServoPosition = this.dynamixel.GetPosition(this.rightServoId);

            RobotiqSModelState state = new RobotiqSModelState();

            if (leftServoPosition < 0)
            {
                      state.Fault = RobotiqSModelFaults.ErrorReadingPosition;
            }
            else
            {
                double leftClosedPercentage =
                      ((double)(leftServoPosition - DefaultLeftFullOpenPosition)) /
                      ((double)(DefaultLeftFullClosePosition - DefaultLeftFullOpenPosition));

                state.FingerAState.Position = (byte)(this.ClampPercentage(leftClosedPercentage) * 255);
            }

            if (rightServoPosition < 0)
            {
                state.Fault = RobotiqSModelFaults.ErrorReadingPosition;
            }
            else
            {
                double rightClosedPercentage =
                    ((double)(DefaultRightFullOpenPosition - rightServoPosition)) /
                    ((double)(DefaultRightFullOpenPosition - DefaultRightFullClosePosition));

                state.FingerBState.Position = (byte)(this.ClampPercentage(rightClosedPercentage) * 255);
            }

            state.IsActivated = true;
            RobotiqSModelStateMessage stateMessage = new RobotiqSModelStateMessage(message, state);

            this.Publisher.Post(stateMessage);
        }

        /// <summary>
        /// Called when agent is deactivated
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();

            this.dynamixel.Dispose();
            this.dynamixel = null;
        }

        /// <summary>
        /// Clamps a percentage to be between 0 and 1
        /// </summary>
        /// <param name="percentage">Value to clamp</param>
        /// <returns>Clamped value.</returns>
        private double ClampPercentage(double percentage)
        {
            if (percentage < 0)
            {
                return 0;
            }

            if (percentage > 1.0)
            {
                return 1.0;
            }

            return percentage;
        }
    }
}
